﻿#include "RobotArm.h"
#include <QDebug>
#include <QFile>
#include <QSharedMemory>
#include <QTcpSocket>
#include <QTimer>
#include <QVariantList>
#include "JsonUtil.h"

static double changeEndian(double d)
{
    UDouble d1, d2, d3;
    memcpy(&d1.d, &d, 8);
    memcpy(&d2.d, &d, 8);
    for (int i = 0; i < 4; ++i) {
        d3.c[i] = d1.c[7 - i];
        d3.c[i + 4] = d2.c[3 - i];
    }
    return d3.d;
}

static void changeEndianOfBytes(quint8* src, int len)
{
    quint8 u8[8] = {0};
    memcpy(u8, src, len);
    for (int i = 0; i < len / 2; ++i) {
        src[i * 2] = u8[i * 2 + 1];
        src[i * 2 + 1] = u8[i * 2];
    }
}

static void changeEndianOfCmdFiled(quint8* cmd)
{
    for (int i = 0; i < 8; ++i) {
        changeEndianOfBytes((quint8*)(cmd + i * 2), 2);
    }

    for (int i = 0; i < 31; ++i) {
        changeEndianOfBytes((quint8*)(cmd + 16 + i * 8), 8);
    }

    changeEndianOfBytes((quint8*)(cmd + 8 * 2 + 31 * 8), 2);
}

RobotArm::RobotArm(int argc, char** argv, QObject* parent)
    : argc_(argc),
      argv_(argv),
      m_usControlMode(1),
      m_usErrClean(0),
      m_usFTMode(1),
      m_usSecMode(0),
      QObject(parent)
{
    m_socket = new QTcpSocket(this);
    m_ros_spin_thread = std::thread{std::bind(&RobotArm::rosSpinThread, this)};
    m_smTcp = new QSharedMemory("sm_tcp", this);

    connect(m_socket, &QTcpSocket::readyRead, this,
            &RobotArm::onSocketReadReady);
    connect(m_socket, &QTcpSocket::connected, this,
            &RobotArm::onSocketConnected);

    m_usLeftEnable = 0;
    m_usRightEnable = 0;
    m_usLeftControlMode = 1;
    m_usRightControlMode = 1;
    m_usLeftErrClean = 0;
    m_usRightErrClean = 0;
    m_usLeftFTMode = 1;
    m_usRightFTMode = 1;
    m_usLeftSecMode = 0;
    m_usRightSecMode = 0;
}

RobotArm::~RobotArm()
{
    rclcpp::shutdown();
}

void RobotArm::connect2Host(const std::string& strIp, unsigned short usPort)
{
    m_strIp = QString::fromStdString(strIp);
    m_usPort = usPort;
    QMetaObject::invokeMethod(this, "connect2Host_p", Qt::QueuedConnection);
}

void RobotArm::connect2Host_p()
{
    m_socket->connectToHost(QHostAddress(m_strIp), m_usPort);
}

void RobotArm::onSocketConnected()
{
    qDebug() << "socket connected";
}

#define SOCK_SIZE_1 (2048)
static quint8 s_buf1[SOCK_SIZE_1] = {0};
static int s_index1 = 0;
void RobotArm::onSocketReadReady()
{
    // QString strRecv = QString(m_socket->readAll());
    // qDebug() << "socket recv: " << strRecv;

    if (s_index1 >= SOCK_SIZE_1 - 200) {
        s_index1 = 0;
    }
    int nRead = m_socket->read((char*)s_buf1 + s_index1, 1024);
    //    qDebug() << "nRead1: " << nRead;
    s_index1 += nRead;
    //    int nParsed = parse1(s_buf1, s_index1);
    int nParsed = parse(s_buf1, s_index1);
    //    qDebug() << __FUNCTION__ << "nParsed2: " << nParsed;
    memmove(s_buf1, s_buf1 + nParsed, s_index1 - nParsed);
    s_index1 -= nParsed;
}

void RobotArm::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

// LeftArm Receiver
int RobotArm::parse(quint8* buf, int len)
{
    if (len < 880) {
        return 0;
    }

    int nStartIndex = 0;

    while (buf[0 + nStartIndex] * 256 + buf[1 + nStartIndex] != 2323) {
        nStartIndex++;
        continue;
    }

    if (nStartIndex + 880 > len) {
        qDebug() << "len is not 880";
        return nStartIndex;
    }

    if (buf[438 + nStartIndex] * 256 + buf[439 + nStartIndex] != 4545) {
        return nStartIndex + 440;
    }

    int nStartIndex2 = 440;

    if (buf[438 + nStartIndex2] * 256 + buf[439 + nStartIndex2] != 8989) {
        return nStartIndex2 + 440;
    }

    MsgRecv msgLeft;
    memset(&msgLeft, 0, sizeof(MsgRecv));
    memcpy(&msgLeft, buf + 4 + nStartIndex, 8 * 20);

    msgLeft.q1 = changeEndian(msgLeft.q1);
    msgLeft.q2 = changeEndian(msgLeft.q2);
    msgLeft.q3 = changeEndian(msgLeft.q3);
    msgLeft.q4 = changeEndian(msgLeft.q4);
    msgLeft.q5 = changeEndian(msgLeft.q5);
    msgLeft.q6 = changeEndian(msgLeft.q6);
    msgLeft.q7 = changeEndian(msgLeft.q7);
    msgLeft.x = changeEndian(msgLeft.x);
    msgLeft.y = changeEndian(msgLeft.y);
    msgLeft.z = changeEndian(msgLeft.z);
    msgLeft.rx = changeEndian(msgLeft.rx);
    msgLeft.ry = changeEndian(msgLeft.ry);
    msgLeft.rz = changeEndian(msgLeft.rz);
    msgLeft.elbow = changeEndian(msgLeft.elbow);
    msgLeft.FT1 = changeEndian(msgLeft.FT1);
    msgLeft.FT2 = changeEndian(msgLeft.FT2);
    msgLeft.FT3 = changeEndian(msgLeft.FT3);
    msgLeft.FT4 = changeEndian(msgLeft.FT4);
    msgLeft.FT5 = changeEndian(msgLeft.FT5);
    msgLeft.FT6 = changeEndian(msgLeft.FT6);

    MsgRecv1 msgLeft1;
    memset(&msgLeft1, 0, sizeof(MsgRecv1));
    memcpy(&msgLeft1, buf + 4 + 8 * 20 + nStartIndex, 2 * 13);

    msgLeft1.jState1 = buf[164 + nStartIndex] * 256 + buf[165 + nStartIndex];
    msgLeft1.jState2 = buf[166 + nStartIndex] * 256 + buf[167 + nStartIndex];
    msgLeft1.jState3 = buf[168 + nStartIndex] * 256 + buf[169 + nStartIndex];
    msgLeft1.jState4 = buf[170 + nStartIndex] * 256 + buf[171 + nStartIndex];
    msgLeft1.jState5 = buf[172 + nStartIndex] * 256 + buf[173 + nStartIndex];
    msgLeft1.jState6 = buf[174 + nStartIndex] * 256 + buf[175 + nStartIndex];
    msgLeft1.jState7 = buf[176 + nStartIndex] * 256 + buf[177 + nStartIndex];
    msgLeft1.Enable = buf[178 + nStartIndex] * 256 + buf[179 + nStartIndex];
    msgLeft1.errorClean = buf[180 + nStartIndex] * 256 + buf[181 + nStartIndex];
    msgLeft1.controlMode =
        buf[182 + nStartIndex] * 256 + buf[183 + nStartIndex];
    msgLeft1.ftMode = buf[184 + nStartIndex] * 256 + buf[185 + nStartIndex];
    msgLeft1.triggerSignal =
        buf[186 + nStartIndex] * 256 + buf[187 + nStartIndex];
    msgLeft1.periodMode = buf[188 + nStartIndex] * 256 + buf[189 + nStartIndex];

    MsgRecv2 msgLeft2;
    memset(&msgLeft2, 0, sizeof(MsgRecv2));
    memcpy(&msgLeft2, buf + 4 + 8 * 20 + 2 * 13 + nStartIndex, 8 * 31);

    msgLeft2.periodValue = changeEndian(msgLeft2.periodValue);
    msgLeft2.toolmass = changeEndian(msgLeft2.toolmass);
    msgLeft2.toolComX = changeEndian(msgLeft2.toolComX);
    msgLeft2.toolComY = changeEndian(msgLeft2.toolComY);
    msgLeft2.toolComZ = changeEndian(msgLeft2.toolComZ);
    msgLeft2.qTarget1 = changeEndian(msgLeft2.qTarget1);
    msgLeft2.qTarget2 = changeEndian(msgLeft2.qTarget2);
    msgLeft2.qTarget3 = changeEndian(msgLeft2.qTarget3);
    msgLeft2.qTarget4 = changeEndian(msgLeft2.qTarget4);
    msgLeft2.qTarget5 = changeEndian(msgLeft2.qTarget5);
    msgLeft2.qTarget6 = changeEndian(msgLeft2.qTarget6);
    msgLeft2.qTarget7 = changeEndian(msgLeft2.qTarget7);
    msgLeft2.xTarget = changeEndian(msgLeft2.xTarget);
    msgLeft2.yTarget = changeEndian(msgLeft2.yTarget);
    msgLeft2.zTarget = changeEndian(msgLeft2.zTarget);
    msgLeft2.rxTarget = changeEndian(msgLeft2.rxTarget);
    msgLeft2.ryTarget = changeEndian(msgLeft2.ryTarget);
    msgLeft2.rzTarget = changeEndian(msgLeft2.rzTarget);
    msgLeft2.elbowTarget = changeEndian(msgLeft2.elbowTarget);
    msgLeft2.kx = changeEndian(msgLeft2.kx);
    msgLeft2.ky = changeEndian(msgLeft2.ky);
    msgLeft2.kz = changeEndian(msgLeft2.kz);
    msgLeft2.krx = changeEndian(msgLeft2.krx);
    msgLeft2.kry = changeEndian(msgLeft2.kry);
    msgLeft2.krz = changeEndian(msgLeft2.krz);
    msgLeft2.cx = changeEndian(msgLeft2.cx);
    msgLeft2.cy = changeEndian(msgLeft2.cy);
    msgLeft2.cz = changeEndian(msgLeft2.cz);
    msgLeft2.crx = changeEndian(msgLeft2.crx);
    msgLeft2.cry = changeEndian(msgLeft2.cry);
    msgLeft2.crz = changeEndian(msgLeft2.crz);

    MsgRecv msgRight;
    memset(&msgRight, 0, sizeof(MsgRecv));
    memcpy(&msgRight, buf + 4 + nStartIndex2, 8 * 20);

    msgRight.q1 = changeEndian(msgRight.q1);
    msgRight.q2 = changeEndian(msgRight.q2);
    msgRight.q3 = changeEndian(msgRight.q3);
    msgRight.q4 = changeEndian(msgRight.q4);
    msgRight.q5 = changeEndian(msgRight.q5);
    msgRight.q6 = changeEndian(msgRight.q6);
    msgRight.q7 = changeEndian(msgRight.q7);
    msgRight.x = changeEndian(msgRight.x);
    msgRight.y = changeEndian(msgRight.y);
    msgRight.z = changeEndian(msgRight.z);
    msgRight.rx = changeEndian(msgRight.rx);
    msgRight.ry = changeEndian(msgRight.ry);
    msgRight.rz = changeEndian(msgRight.rz);
    msgRight.elbow = changeEndian(msgRight.elbow);
    msgRight.FT1 = changeEndian(msgRight.FT1);
    msgRight.FT2 = changeEndian(msgRight.FT2);
    msgRight.FT3 = changeEndian(msgRight.FT3);
    msgRight.FT4 = changeEndian(msgRight.FT4);
    msgRight.FT5 = changeEndian(msgRight.FT5);
    msgRight.FT6 = changeEndian(msgRight.FT6);

    MsgRecv1 msgRight1;
    memset(&msgRight1, 0, sizeof(MsgRecv1));
    memcpy(&msgRight1, buf + 4 + 8 * 20 + nStartIndex2, 2 * 13);

    msgRight1.jState1 = buf[164 + nStartIndex2] * 256 + buf[165 + nStartIndex2];
    msgRight1.jState2 = buf[166 + nStartIndex2] * 256 + buf[167 + nStartIndex2];
    msgRight1.jState3 = buf[168 + nStartIndex2] * 256 + buf[169 + nStartIndex2];
    msgRight1.jState4 = buf[170 + nStartIndex2] * 256 + buf[171 + nStartIndex2];
    msgRight1.jState5 = buf[172 + nStartIndex2] * 256 + buf[173 + nStartIndex2];
    msgRight1.jState6 = buf[174 + nStartIndex2] * 256 + buf[175 + nStartIndex2];
    msgRight1.jState7 = buf[176 + nStartIndex2] * 256 + buf[177 + nStartIndex2];
    msgRight1.Enable = buf[178 + nStartIndex2] * 256 + buf[179 + nStartIndex2];
    msgRight1.errorClean =
        buf[180 + nStartIndex2] * 256 + buf[181 + nStartIndex2];
    msgRight1.controlMode =
        buf[182 + nStartIndex2] * 256 + buf[183 + nStartIndex2];
    msgRight1.ftMode = buf[184 + nStartIndex2] * 256 + buf[185 + nStartIndex2];
    msgRight1.triggerSignal =
        buf[186 + nStartIndex2] * 256 + buf[187 + nStartIndex2];
    msgRight1.periodMode =
        buf[188 + nStartIndex2] * 256 + buf[189 + nStartIndex2];

    MsgRecv2 msgRight2;
    memset(&msgRight2, 0, sizeof(MsgRecv2));
    memcpy(&msgRight2, buf + 4 + 8 * 20 + 2 * 13 + nStartIndex2, 8 * 31);

    msgRight2.periodValue = changeEndian(msgRight2.periodValue);
    msgRight2.toolmass = changeEndian(msgRight2.toolmass);
    msgRight2.toolComX = changeEndian(msgRight2.toolComX);
    msgRight2.toolComY = changeEndian(msgRight2.toolComY);
    msgRight2.toolComZ = changeEndian(msgRight2.toolComZ);
    msgRight2.qTarget1 = changeEndian(msgRight2.qTarget1);
    msgRight2.qTarget2 = changeEndian(msgRight2.qTarget2);
    msgRight2.qTarget3 = changeEndian(msgRight2.qTarget3);
    msgRight2.qTarget4 = changeEndian(msgRight2.qTarget4);
    msgRight2.qTarget5 = changeEndian(msgRight2.qTarget5);
    msgRight2.qTarget6 = changeEndian(msgRight2.qTarget6);
    msgRight2.qTarget7 = changeEndian(msgRight2.qTarget7);
    msgRight2.xTarget = changeEndian(msgRight2.xTarget);
    msgRight2.yTarget = changeEndian(msgRight2.yTarget);
    msgRight2.zTarget = changeEndian(msgRight2.zTarget);
    msgRight2.rxTarget = changeEndian(msgRight2.rxTarget);
    msgRight2.ryTarget = changeEndian(msgRight2.ryTarget);
    msgRight2.rzTarget = changeEndian(msgRight2.rzTarget);
    msgRight2.elbowTarget = changeEndian(msgRight2.elbowTarget);
    msgRight2.kx = changeEndian(msgRight2.kx);
    msgRight2.ky = changeEndian(msgRight2.ky);
    msgRight2.kz = changeEndian(msgRight2.kz);
    msgRight2.krx = changeEndian(msgRight2.krx);
    msgRight2.kry = changeEndian(msgRight2.kry);
    msgRight2.krz = changeEndian(msgRight2.krz);
    msgRight2.cx = changeEndian(msgRight2.cx);
    msgRight2.cy = changeEndian(msgRight2.cy);
    msgRight2.cz = changeEndian(msgRight2.cz);
    msgRight2.crx = changeEndian(msgRight2.crx);
    msgRight2.cry = changeEndian(msgRight2.cry);
    msgRight2.crz = changeEndian(msgRight2.crz);

    if (!m_smTcp->isAttached()) {
        m_smTcp->attach();
    }

    m_smTcp->lock();
    memcpy((char*)m_smTcp->data(), &msgLeft, sizeof(MsgRecv));
    memcpy((char*)m_smTcp->data() + sizeof(MsgRecv), &msgLeft1,
           sizeof(MsgRecv1));
    memcpy((char*)m_smTcp->data() + sizeof(MsgRecv) + sizeof(MsgRecv1),
           &msgLeft2, sizeof(MsgRecv2));

    memcpy((char*)m_smTcp->data() + sizeof(MsgRecv) + sizeof(MsgRecv1) +
               sizeof(MsgRecv2),
           &msgRight, sizeof(MsgRecv));
    memcpy((char*)m_smTcp->data() + sizeof(MsgRecv) + sizeof(MsgRecv1) +
               sizeof(MsgRecv2) + sizeof(MsgRecv),
           &msgRight1, sizeof(MsgRecv1));
    memcpy((char*)m_smTcp->data() + sizeof(MsgRecv) + sizeof(MsgRecv1) +
               sizeof(MsgRecv2) + sizeof(MsgRecv) + sizeof(MsgRecv1),
           &msgRight2, sizeof(MsgRecv2));

    m_smTcp->unlock();

    m_robotCmdNode->pubJsonBa(
        "tcp", 2 * (sizeof(MsgRecv) + sizeof(MsgRecv1) + sizeof(MsgRecv2)));

    // emit sigRecvMsg1(msgLeft.q1, msgLeft.q2, msgLeft.q3, msgLeft.q4,
    // msgLeft.q5, msgLeft.q6, msgLeft.q7,
    //     msgLeft.x, msgLeft.y, msgLeft.z, msgLeft.rx, msgLeft.ry, msgLeft.rz,
    //     msgLeft.elbow, msgLeft.FT1, msgLeft.FT2, msgLeft.FT3, msgLeft.FT4,
    //     msgLeft.FT5, msgLeft.FT6, msgLeft1.jState1, msgLeft1.jState2,
    //     msgLeft1.jState3, msgLeft1.jState4, msgLeft1.jState5,
    //     msgLeft1.jState6, msgLeft1.jState7, msgLeft1.Enable,
    //     msgLeft1.errorClean, msgLeft1.controlMode, msgLeft1.ftMode,
    //     msgLeft1.triggerSignal, msgLeft1.periodMode, msgLeft2.periodValue,
    //     msgLeft2.toolmass, msgLeft2.toolSizeX, msgLeft2.toolSizeY,
    //     msgLeft2.toolSizeZ, msgLeft2.qTarget1, msgLeft2.qTarget2,
    //     msgLeft2.qTarget3, msgLeft2.qTarget4, msgLeft2.qTarget5,
    //     msgLeft2.qTarget6, msgLeft2.qTarget7, msgLeft2.xTarget,
    //     msgLeft2.yTarget, msgLeft2.zTarget, msgLeft2.rxTarget,
    //     msgLeft2.ryTarget, msgLeft2.rzTarget, msgLeft2.elbowTarget,
    //     msgLeft2.kx, msgLeft2.ky, msgLeft2.kz, msgLeft2.krx, msgLeft2.kry,
    //     msgLeft2.krz, msgLeft2.cx, msgLeft2.cy, msgLeft2.cz, msgLeft2.crx,
    //     msgLeft2.cry, msgLeft2.crz);

    // emit sigRecvMsgL(msgLeft.x, msgLeft.y, msgLeft.z, msgLeft.rx, msgLeft.ry,
    // msgLeft.rz);

    // emit sigRecvMsg2(msgRight.q1, msgRight.q2, msgRight.q3, msgRight.q4,
    // msgRight.q5, msgRight.q6, msgRight.q7,
    //     msgRight.x, msgRight.y, msgRight.z, msgRight.rx, msgRight.ry,
    //     msgRight.rz, msgRight.elbow, msgRight.FT1, msgRight.FT2,
    //     msgRight.FT3, msgRight.FT4, msgRight.FT5, msgRight.FT6,
    //     msgRight1.jState1, msgRight1.jState2, msgRight1.jState3,
    //     msgRight1.jState4, msgRight1.jState5, msgRight1.jState6,
    //     msgRight1.jState7, msgRight1.Enable, msgRight1.errorClean,
    //     msgRight1.controlMode, msgRight1.ftMode, msgRight1.triggerSignal,
    //     msgRight1.periodMode, msgRight2.periodValue, msgRight2.toolmass,
    //     msgRight2.toolSizeX, msgRight2.toolSizeY, msgRight2.toolSizeZ,
    //     msgRight2.qTarget1, msgRight2.qTarget2, msgRight2.qTarget3,
    //     msgRight2.qTarget4, msgRight2.qTarget5, msgRight2.qTarget6,
    //     msgRight2.qTarget7, msgRight2.xTarget, msgRight2.yTarget,
    //     msgRight2.zTarget, msgRight2.rxTarget, msgRight2.ryTarget,
    //     msgRight2.rzTarget, msgRight2.elbowTarget, msgRight2.kx,
    //     msgRight2.ky, msgRight2.kz, msgRight2.krx, msgRight2.kry,
    //     msgRight2.krz, msgRight2.cx, msgRight2.cy, msgRight2.cz,
    //     msgRight2.crx, msgRight2.cry, msgRight2.crz);

    // emit sigRecvMsgR(msgRight.x, msgRight.y, msgRight.z, msgRight.rx,
    // msgRight.ry, msgRight.rz);

    return nStartIndex2 + 440;
}

void RobotArm::setLeftEnable(quint16 e)
{
    m_usLeftEnable = e;
    //    qDebug() << "s_leftEnable = " << s_leftEnable;

    qDebug() << __FUNCTION__ << "left enable 1: " << m_usLeftEnable;
    qDebug() << __FUNCTION__ << "right enable 1: " << m_usRightEnable;
}

void RobotArm::setRightEnable(quint16 e)
{
    m_usRightEnable = e;
    //    qDebug() << "s_rightEnable = " << s_rightEnable;

    qDebug() << __FUNCTION__ << "left enable 1: " << m_usLeftEnable;
    qDebug() << __FUNCTION__ << "right enable 1: " << m_usRightEnable;
}

// change0405
void RobotArm::setLeftControlMode(quint16 e)
{
    m_usLeftControlMode = e;
    //    qDebug() << "s_leftControlMode = " << s_leftControlMode;
}
void RobotArm::setRightControlMode(quint16 e)
{
    m_usRightControlMode = e;
    //    qDebug() << "s_rightControlMode = " << s_rightControlMode;
}
void RobotArm::setLeftErrClean(quint16 e)
{
    m_usLeftErrClean = e;
    //    qDebug() << "s_leftErrClean = " << s_leftErrClean;
}
void RobotArm::setRightErrClean(quint16 e)
{
    m_usRightErrClean = e;
    //    qDebug() << "s_rightErrClean = " << s_rightErrClean;
}
void RobotArm::setLeftFTMode(quint16 e)
{
    m_usLeftFTMode = e;
    //    qDebug() << "s_leftFTMode = " << s_leftFTMode;
}
void RobotArm::setRightFTMode(quint16 e)
{
    m_usRightFTMode = e;
    //    qDebug() << "s_rightFTMode = " << s_rightFTMode;
}
void RobotArm::setLeftSecMode(quint16 e)
{
    m_usLeftSecMode = e;
    //    qDebug() << "s_leftSecMode = " << s_leftSecMode;
}
void RobotArm::setRightSecMode(quint16 e)
{
    m_usRightSecMode = e;
    //    qDebug() << "s_rightSecMode = " << s_rightSecMode;
}

void RobotArm::call_writeDuoArmDirect(const QVariantMap& leftCmd,
                                      const QVariantMap& rightCmd)
{
    QMetaObject::invokeMethod(this, "writeDuoArmDirect", Qt::QueuedConnection,
                              Q_ARG(QVariantMap, leftCmd),
                              Q_ARG(QVariantMap, rightCmd));
}

void RobotArm::writeDuoArmDirect(const QVariantMap& leftCmd,
                                 const QVariantMap& rightCmd)
{

    RCLCPP_INFO_STREAM(m_robotCmdNode->get_logger(), __FUNCTION__
                                               << "left enable 1: " << m_usLeftEnable);

    RCLCPP_INFO_STREAM(m_robotCmdNode->get_logger(), __FUNCTION__
                                               << "right enable 1: " << m_usRightEnable);

    Cmd leftArm;
    leftArm.head = 1212;
    leftArm.cx = leftCmd.value("cx", 500).toDouble();
    leftArm.cy = leftCmd.value("cy", 500).toDouble();
    leftArm.cz = leftCmd.value("cz", 500).toDouble();
    leftArm.kx = leftCmd.value("kx", 500).toDouble();
    leftArm.ky = leftCmd.value("ky", 500).toDouble();
    leftArm.kz = leftCmd.value("kz", 500).toDouble();

    leftArm.crx = leftCmd.value("crx", 20).toDouble();
    leftArm.cry = leftCmd.value("cry", 30).toDouble();
    leftArm.crz = leftCmd.value("crz", 20).toDouble();
    leftArm.krx = leftCmd.value("krx", 20).toDouble();
    leftArm.kry = leftCmd.value("kry", 30).toDouble();
    leftArm.krz = leftCmd.value("krz", 20).toDouble();

    leftArm.q1 = leftCmd.value("q1", 0.734).toDouble();
    leftArm.q2 = leftCmd.value("q2", 1.006).toDouble();
    leftArm.q3 = leftCmd.value("q3", 0.585).toDouble();
    leftArm.q4 = leftCmd.value("q4", -2.269).toDouble();
    leftArm.q5 = leftCmd.value("q5", -0.437).toDouble();
    leftArm.q6 = leftCmd.value("q6", 2.109).toDouble();
    leftArm.q7 = leftCmd.value("q7", -0.215).toDouble();

    leftArm.enable = m_usLeftEnable;
    leftArm.ec = m_usLeftErrClean;
    leftArm.FT_MODE = m_usLeftFTMode;
    leftArm.PERIOD_MODE = m_usLeftSecMode;
    leftArm.period = leftCmd.value("periodValue", 40).toDouble();

    leftArm.weight_tool = leftCmd.value("toolWeight", 0).toDouble();
    leftArm.x_size = leftCmd.value("toolComX", 0).toDouble();
    leftArm.y_size = leftCmd.value("toolComY", 0).toDouble();
    leftArm.z_size = leftCmd.value("toolComZ", 0).toDouble();

    leftArm.MODE = m_usLeftControlMode;
    leftArm.moving_trigger = leftCmd.value("trigger", 0).toUInt();

    leftArm.x2 = leftCmd.value("x2", 0.4).toDouble();
    leftArm.y2 = leftCmd.value("y2", 0.2).toDouble();
    leftArm.z2 = leftCmd.value("z2", -0.1).toDouble();
    leftArm.rx2 = leftCmd.value("rx2", 0).toDouble();
    leftArm.ry2 = leftCmd.value("ry2", 1).toDouble();
    leftArm.rz2 = leftCmd.value("rz2", 0).toDouble();
    leftArm.elbow2 = leftCmd.value("elbow2", 0.904).toDouble();

    Cmd rightArm;
    rightArm.head = 2323;
    rightArm.cx = rightCmd.value("cx", 500).toDouble();
    rightArm.cy = rightCmd.value("cy", 500).toDouble();
    rightArm.cz = rightCmd.value("cz", 500).toDouble();
    rightArm.kx = rightCmd.value("kx", 500).toDouble();
    rightArm.ky = rightCmd.value("ky", 500).toDouble();
    rightArm.kz = rightCmd.value("kz", 500).toDouble();
    rightArm.crx = rightCmd.value("crx", 20).toDouble();
    rightArm.cry = rightCmd.value("cry", 30).toDouble();
    rightArm.crz = rightCmd.value("crz", 20).toDouble();
    rightArm.krx = rightCmd.value("krx", 20).toDouble();
    rightArm.kry = rightCmd.value("kry", 30).toDouble();
    rightArm.krz = rightCmd.value("krz", 20).toDouble();
    rightArm.q1 = rightCmd.value("q1", -0.734).toDouble();
    rightArm.q2 = rightCmd.value("q2", 1.006).toDouble();
    rightArm.q3 = rightCmd.value("q3", -0.585).toDouble();
    rightArm.q4 = rightCmd.value("q4", -2.269).toDouble();
    rightArm.q5 = rightCmd.value("q5", 0.437).toDouble();
    rightArm.q6 = rightCmd.value("q6", 2.109).toDouble();
    rightArm.q7 = rightCmd.value("q7", 0.215).toDouble();

    rightArm.enable = m_usRightEnable;
    rightArm.ec = m_usRightErrClean;
    rightArm.FT_MODE = m_usRightFTMode;
    rightArm.PERIOD_MODE = m_usRightSecMode;

    rightArm.period = rightCmd.value("periodValue", 40).toDouble();

    rightArm.weight_tool = rightCmd.value("toolWeight", 0.0).toDouble();
    rightArm.x_size = rightCmd.value("toolComX", 0.0).toDouble();
    rightArm.y_size = rightCmd.value("toolComY", 0.0).toDouble();
    rightArm.z_size = rightCmd.value("toolComZ", 0.0).toDouble();

    rightArm.MODE = m_usRightControlMode;
    rightArm.moving_trigger = rightCmd.value("trigger", 0).toUInt();

    rightArm.x2 = rightCmd.value("x2", 0.4).toDouble();
    rightArm.y2 = rightCmd.value("y2", -0.2).toDouble();
    rightArm.z2 = rightCmd.value("z2", -0.1).toDouble();
    rightArm.rx2 = rightCmd.value("rx2", 0).toDouble();
    rightArm.ry2 = rightCmd.value("ry2", 1).toDouble();
    rightArm.rz2 = rightCmd.value("rz2", 0).toDouble();
    rightArm.elbow2 = rightCmd.value("elbow2", 0.904).toDouble();
    //    qDebug() << "\n\n\n";
    //    qDebug() << "leftArm.weight_tool: " << leftArm.weight_tool <<
    //    "rightArm.weight_tool: " << rightArm.weight_tool;

    quint8 buf[2 * sizeof(Cmd)];
    memset(buf, 0, 2 * sizeof(Cmd));
    memcpy(buf, &leftArm, sizeof(Cmd));
    memcpy(buf + sizeof(Cmd), &rightArm, sizeof(Cmd));

    Cmd& leftTmp = *(Cmd*)buf;
    Cmd& rightTmp = *(Cmd*)(buf + sizeof(Cmd));

    changeEndianOfCmdFiled((quint8*)(&leftTmp));
    changeEndianOfCmdFiled((quint8*)(&rightTmp));

    RCLCPP_INFO_STREAM(m_robotCmdNode->get_logger(), __FUNCTION__
                                               << "leftArm.moving_trigger: " << leftArm.moving_trigger << "  rightArm.moving_trigger:" << rightArm.moving_trigger);

    qint64 nWrite = 0;
    while (nullptr != m_socket && m_socket->isValid() &&
           nWrite < 2 * sizeof(Cmd)) {
        nWrite += m_socket->write((const char*)buf + nWrite,
                                  2 * sizeof(Cmd) - nWrite);
        qDebug() << __FUNCTION__ << " total: " << 2 * sizeof(Cmd)
                 << "  ,write: " << nWrite;

        RCLCPP_INFO_STREAM(m_robotCmdNode->get_logger(), __FUNCTION__
                                                   << " total: " << 2 * sizeof(Cmd)
                                                   << "  ,write: " << nWrite);
    }
}
